//
// Created by codex on 2024/7/19.
//
#include<stdio.h>
#include<math.h>

#define Max 20
#define INF 0xcffffff



int map[17][2]={{0,0},{30,375},{200,375},{340,375},{465,375},{30,230},{140,230},{200,230}
        ,{340,230},{465,230},{170,200},{170,80},{35,35},{135,35},{340,25},{465,25}};
uint8_t fs_x,fs_y;
uint8_t waypoint[16][7]={  {0},
                       {12, 5, 1},
                       {11, 10, 7, 2},
                       {11, 10, 7, 8, 3},
                       {11, 10, 7, 8, 9, 4},
                       {12, 5},
                       {11, 10, 6},
                       {11, 10, 7},
                       {11, 10, 7, 8},
                       {11, 10, 7, 8, 9},
                       {11, 10},
                       {11},
                       {12},
                       {14},
                       {14, 15}
};
uint8_t waypoint_now[10];
uint8_t waypoint_calc(unsigned char fs_x,unsigned char fs_y)
{

    int minlen=10000,len_t,minnum=0;
    for(int i=1;i<=15;i++)
    {
        len_t=(int)sqrt((map[i][0]-fs_x)*(map[i][0]-fs_x)+(map[i][1]-fs_y)*(map[i][1]-fs_y));
        if(minlen>len_t)
        {
            minnum=i;
            minlen=len_t;
        }
    }
    return minnum;
//    printf("%d 路径为：",minnum);
//    for(int i=0;waypoint[minnum][i]!=0;i++)
//    {
//        printf("%d->",waypoint[minnum][i]);
//    }
}